#ifndef __COSTMAP_BASE_H__
#define __COSTMAP_BASE_H__

#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/PointCloud2.h>

#include "core/common/core.h"
#include<unordered_map>
using namespace std;

namespace costmap_2d{

    static const unsigned char UNKNOWN = 255;           //未知数据
    static const unsigned char LETHAL_OBSTACLE = 254;   //致命障碍物
    static const unsigned char FREE_SPACE = 0;          //空闲区域
    static const double MAX_INFLASTION_DISTANCE = 2.0;  //最远膨胀距离 单位米

    template <typename T>
    void Clear(vector<T>& a){
        vector<T> temp;
        a.swap(temp);
    }

    struct GridCell{
        int x;     //当前坐标
        int y;
        int index;
        int sx;    //膨胀的障碍物原始坐标
        int sy;
        unsigned char cost;
    };

    class Costmap{
        public:
            Costmap();
            ~Costmap();

            // 添加地图以及相关数据
            void setMap(nav_msgs::OccupancyGrid::Ptr map_ptr);

            void indexToMap(int index, int& mx, int& my); 
            int mapToIndex(int mx, int my);

            void mapToWorld(int mx, int my, double& wx, double& wy);
            void worldToMap(double wx, double wy, int& mx, int& my);

            void opencvMapToWorld(int mx, int my, double& wx, double& wy);
            void worldToOpencvMap(double wx, double wy, int& mx, int& my);

            bool isValid(int mx, int my);
            bool isValid(int index);

            // 是否为障碍物
            bool valueIsValid(int mx, int my);
            unsigned char getCost(int mx, int my);
            unsigned char getCost(int index);

            public:
                nav_msgs::OccupancyGrid::Ptr mapOriginPtr;         //全局静态地图原始数据
                nav_msgs::MapMetaData map_metadata;
                vector<unsigned char> costMap;
                string frameId;
    };

}; //costmap_2d

#endif